#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include "Preferences.h"
#include "string.h"
#include "ArduinoJson.h"


#define PWM1_GPIO    32
#define PWM2_GPIO    33
#define PWM3_GPIO    5
#define PWM4_GPIO    18
#define PWM5_GPIO    19
#define PWM6_GPIO    14
#define mmwavetrig_gpio 16
#define PWM1_ch      0
#define PWM2_ch      1
#define PWM3_ch      2
#define PWM4_ch      3
#define PWM5_ch      4
#define PWM6_ch      5
#define MOTO1pwm PWM1_GPIO
#define MOTO2pwm PWM3_GPIO
#define MOTO1dir PWM2_GPIO
#define MOTO2dir PWM4_GPIO
#define MOTO1en  PWM6_GPIO
#define MOTO2en  PWM5_GPIO

hw_timer_t * timer0= NULL;
hw_timer_t * timer1= NULL;

#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

StaticJsonDocument<600> bleTxjson,bleRxjson;


BLEServer *pServer = NULL;
BLECharacteristic * pTxCharacteristic;
BLECharacteristic * pRxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
Preferences pref;
uint64_t xray_plusenum=14000,xray_fre=30000,yray_plusenum=40,yray_fre=100,yray_times=200,sample_duty=0;
int64_t now_worktimes=yray_times;
int64_t xray_location=0,yray_location=0,xray_locationset=0,yray_locationset=0;
bool rsetlocation_state=false,xray_workstate=false,yray_workstate=false;
volatile uint32_t isrCounter = 0;
volatile uint32_t lastIsrAt = 0;
bool MOTO1pwmstate=false;
bool MOTO2pwmstate=false;

volatile SemaphoreHandle_t timer0Semaphore;
volatile SemaphoreHandle_t timer1Semaphore;
volatile SemaphoreHandle_t mmwavetriggeron;
volatile SemaphoreHandle_t mmwavetriggeroff;
portMUX_TYPE timer0Mux = portMUX_INITIALIZER_UNLOCKED;
portMUX_TYPE timer1Mux = portMUX_INITIALIZER_UNLOCKED;
void setpwmfre(hw_timer_t * timer,uint64_t fre);

class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};

class CharacteristicTxCallback: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();
      if (rxValue.length() > 0) {
        Serial.print("Received Value: ");
        String rxdata=rxValue.c_str();
        Serial.println(rxdata);
        if(rxValue.find_first_of("1")==0){
          if((now_worktimes>=yray_times)&&(xray_location==0)&&(yray_location==0)){//在停止运行后并且复位后才能控制进行重新运行
          now_worktimes=0;//开始运行
          xSemaphoreGiveFromISR(timer1Semaphore, NULL);
          }
          // Serial.println("state1");
          // portENTER_CRITICAL_ISR(&timer0Mux);
          // xray_locationset=10000;
          // xray_workstate=true;
          // portEXIT_CRITICAL_ISR(&timer0Mux);
        }
        if(rxValue.find_first_of("2")==0){
          now_worktimes=yray_times;//停止运行，并复位。
          xSemaphoreGiveFromISR(timer1Semaphore, NULL);
          // Serial.println("state1");
          // portENTER_CRITICAL_ISR(&timer0Mux);
          // xray_locationset=-10000;
          // xray_workstate=true;
          // portEXIT_CRITICAL_ISR(&timer0Mux);
        }
      }
    }
};

class CharacteristicRxCallback: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();

      if (rxValue.length() > 0) {
        Serial.print("Received Value: ");
        String rxdata=rxValue.c_str();
        Serial.println(rxdata);
        // if(rxValue.find_first_of("1")==0){
        //   portENTER_CRITICAL_ISR(&timer0Mux);
        //   xray_location=1000;
        //   portEXIT_CRITICAL_ISR(&timer0Mux);
        //   xSemaphoreGiveFromISR(timer1Semaphore, NULL);
        // }
        // if(rxValue.find_first_of("2")==0){
        //   portENTER_CRITICAL_ISR(&timer0Mux);
        //   xray_location=-1000;
        //   portEXIT_CRITICAL_ISR(&timer0Mux);
        //   xSemaphoreGiveFromISR(timer1Semaphore, NULL);
        // }
        // if(rxValue.find_first_of("3")==0){
        //   portENTER_CRITICAL_ISR(&timer1Mux);
        //   yray_location=1000;
        //   portEXIT_CRITICAL_ISR(&timer1Mux);
        //   xSemaphoreGiveFromISR(timer0Semaphore, NULL);
        // }
        // if(rxValue.find_first_of("4")==0){
        //   portENTER_CRITICAL_ISR(&timer1Mux);
        //   yray_location=-1000;
        //   portEXIT_CRITICAL_ISR(&timer1Mux);
        //   xSemaphoreGiveFromISR(timer0Semaphore, NULL);
        // }
        bleRxjson.clear();
        DeserializationError error = deserializeJson(bleRxjson,rxdata); 
        if((!error)&&(now_worktimes>=yray_times)&&(xray_location==0)&&(yray_location==0)){//在停止运行后，并且复位后才能进行重新运行
          if(bleRxjson.containsKey("xray_plusenum")){
            xray_plusenum=bleRxjson["xray_plusenum"];
          }
          if(bleRxjson.containsKey("xray_fre")){
            xray_fre=bleRxjson["xray_fre"];
              setpwmfre(timer0,xray_fre);
          }
          if(bleRxjson.containsKey("yray_plusenum")){
            yray_plusenum=bleRxjson["yray_plusenum"];
          }
          if(bleRxjson.containsKey("yray_fre")){
            yray_fre=bleRxjson["yray_fre"];
            setpwmfre(timer1,yray_fre);
          }
          if(bleRxjson.containsKey("yray_times")){
            yray_times=bleRxjson["yray_times"];
          }
          if(bleRxjson.containsKey("sample_duty")){
            sample_duty=bleRxjson["sample_duty"];
          }
          if(bleRxjson.containsKey("xray_location")){
            int64_t setlocation = bleRxjson["xray_location"];
            if(setlocation!=0){
              portENTER_CRITICAL_ISR(&timer0Mux);
              xray_location=setlocation;
              portEXIT_CRITICAL_ISR(&timer0Mux);
              xSemaphoreGiveFromISR(timer1Semaphore, NULL);
            }
          }
          if(bleRxjson.containsKey("yray_location")){
            int64_t setlocation = bleRxjson["yray_location"];
            if(setlocation!=0){
              portENTER_CRITICAL_ISR(&timer1Mux);
              yray_location=setlocation;
              portEXIT_CRITICAL_ISR(&timer1Mux);
              xSemaphoreGiveFromISR(timer0Semaphore, NULL);
            }
          }
          pref.begin("configuration");
          pref.putLong64("xray_plusenum",xray_plusenum);
          pref.putLong64("xray_fre",xray_fre);
          pref.putLong64("yray_plusenum",yray_plusenum);
          pref.putLong64("yray_fre",yray_fre);
          pref.putLong64("yray_times",yray_times);
          pref.putLong64("sample_duty",sample_duty);
          pref.end();
          bleRxjson.clear();
          bleRxjson["xray_plusenum"]=xray_plusenum;
          bleRxjson["xray_fre"]=xray_fre;
          bleRxjson["yray_plusenum"]=yray_plusenum;
          bleRxjson["yray_fre"]=yray_fre;
          bleRxjson["yray_times"]=yray_times;
          bleRxjson["sample_duty"]=sample_duty;
          char state_doc[measureJson(bleRxjson) + 1];
          serializeJson(bleRxjson, state_doc, measureJson(bleRxjson) + 1);
          pRxCharacteristic->setValue(state_doc);
        }
        else{
          Serial.printf("Deserialization error:%d\n",error);
        }
      }
    }
};

/**
 * @brief 
 * x轴电机控制pwm定时器中断
 * 
 */
void IRAM_ATTR onTimer0(){
  // Increment the counter and set the time of ISR
  // Give a semaphore that we can check in the loop
  int64_t movepluse=0;
  bool movestate=false;
  portENTER_CRITICAL_ISR(&timer0Mux);
  movepluse=xray_locationset;
  movestate=xray_workstate;
  portEXIT_CRITICAL_ISR(&timer0Mux);
  if(movestate){
    if(movepluse>0){
      digitalWrite(MOTO1dir,HIGH);
      if(MOTO1pwmstate){
        digitalWrite(MOTO1pwm,LOW);
        MOTO1pwmstate=false;
        portENTER_CRITICAL_ISR(&timer0Mux);
        xray_locationset-=1;
        xray_location+=1;
        portEXIT_CRITICAL_ISR(&timer0Mux);
        if(sample_duty==0){
          if(xray_location==1000) xSemaphoreGiveFromISR(mmwavetriggeron, NULL);
          if(xray_location==xray_plusenum-1000) xSemaphoreGiveFromISR(mmwavetriggeroff, NULL);
        }else{
          if((xray_location>=10000)&&(xray_location<=xray_plusenum-10000)&&((xray_location-10000)%sample_duty==0)) xSemaphoreGiveFromISR(mmwavetriggeron, NULL);
        }
      }
      else{
        digitalWrite(MOTO1pwm,HIGH);
        MOTO1pwmstate=true;
      }
    }
    else if(movepluse<0){
      digitalWrite(MOTO1dir,LOW);
      if(MOTO1pwmstate){
        digitalWrite(MOTO1pwm,LOW);
        MOTO1pwmstate=false;
        portENTER_CRITICAL_ISR(&timer0Mux);
        xray_locationset+=1;
        xray_location-=1;
        portEXIT_CRITICAL_ISR(&timer0Mux);
        if(sample_duty==0){
          if(xray_location==1000) xSemaphoreGiveFromISR(mmwavetriggeroff, NULL);
          if(xray_location==xray_plusenum-1000) xSemaphoreGiveFromISR(mmwavetriggeron, NULL);
        }else{
          if((xray_location>=10000)&&(xray_location<=xray_plusenum-10000)&&((xray_location-10000)%sample_duty==0)) xSemaphoreGiveFromISR(mmwavetriggeron, NULL);
        }
      }
      else{
        digitalWrite(MOTO1pwm,HIGH);
        MOTO1pwmstate=true;
      }
    }
    else{
      portENTER_CRITICAL_ISR(&timer0Mux);
      xray_workstate=false;
      now_worktimes+=1;
      portEXIT_CRITICAL_ISR(&timer0Mux);
      xSemaphoreGiveFromISR(timer0Semaphore, NULL);
    }
  }
  // It is safe to use digitalRead/Write here if you want to toggle an output
  // xSemaphoreGiveFromISR(timer0Semaphore, NULL);
}

/**
 * @brief 
 * y轴电机pwm定时器中断
 */
void IRAM_ATTR onTimer1(){
  // Increment the counter and set the time of ISR
  // Give a semaphore that we can check in the loop
  int64_t movepluse=0;
  bool movestate=false;
  portENTER_CRITICAL_ISR(&timer1Mux);
  movepluse=yray_locationset;
  movestate=yray_workstate;
  portEXIT_CRITICAL_ISR(&timer1Mux);
  if(movestate){
    if(movepluse>0){
      digitalWrite(MOTO2dir,LOW);
      if(MOTO2pwmstate){
        digitalWrite(MOTO2pwm,LOW);
        MOTO2pwmstate=false;
        portENTER_CRITICAL_ISR(&timer1Mux);
        yray_locationset-=1;
        yray_location+=1;
        portEXIT_CRITICAL_ISR(&timer1Mux);
      }
      else{
        digitalWrite(MOTO2pwm,HIGH);
        MOTO2pwmstate=true;
      }
    }
    else if(movepluse<0){
      digitalWrite(MOTO2dir,HIGH);
      if(MOTO2pwmstate){
        digitalWrite(MOTO2pwm,LOW);
        MOTO2pwmstate=false;
        portENTER_CRITICAL_ISR(&timer1Mux);
        yray_locationset+=1;
        yray_location-=1;
        portEXIT_CRITICAL_ISR(&timer1Mux);
      }
      else{
        digitalWrite(MOTO2pwm,HIGH);
        MOTO2pwmstate=true;
      }
    }
    else{
      digitalWrite(MOTO2en,HIGH);                        //电机2取消使能，防止噪声导致抖动
      portENTER_CRITICAL_ISR(&timer1Mux);
      yray_workstate=false;
      portEXIT_CRITICAL_ISR(&timer1Mux);
      xSemaphoreGiveFromISR(timer1Semaphore, NULL);
    }
  }
  // It is safe to use digitalRead/Write here if you want to toggle an output
  // xSemaphoreGiveFromISR(timer1Semaphore, NULL);
}
/**
 * @brief 
 * sample_duty==0的时候为在需要采集的时候一直为高电平，如果不为0 则是脉冲采集的方式。
 * 
 * @param pvParameters 
 */
void mmwavetrigtask(void *pvParameters){
  while(1){
    if(sample_duty==0){
      if (xSemaphoreTake(mmwavetriggeron, portMAX_DELAY ) == pdTRUE){
        digitalWrite(mmwavetrig_gpio,HIGH);
      }
      if (xSemaphoreTake(mmwavetriggeroff, portMAX_DELAY ) == pdTRUE){
        digitalWrite(mmwavetrig_gpio,LOW);
      }
    }else{
      if (xSemaphoreTake(mmwavetriggeron, portMAX_DELAY ) == pdTRUE){
        // Serial.println("mmwave");
        digitalWrite(mmwavetrig_gpio,HIGH);
        ets_delay_us(3);
        digitalWrite(mmwavetrig_gpio,LOW);
      }
    }


  }
}

/**
 * @brief 
 * 蓝牙状态控制函数，处理蓝牙连接断开
 * 
 * @param pvParameters 
 */
void ble_handletask(void *pvParameters){
for(;;){
      if (deviceConnected) {
      delay(10); // bluetooth stack will go into congestion, if too many packets are sent
      }
      // disconnecting
      if (!deviceConnected && oldDeviceConnected) {
          delay(500); // give the bluetooth stack the chance to get things ready
          pServer->startAdvertising(); // restart advertising
          Serial.println("start advertising");
          oldDeviceConnected = deviceConnected;
      }
      // connecting
      if (deviceConnected && !oldDeviceConnected) {
      // do stuff here on connecting
          oldDeviceConnected = deviceConnected;
      }
      delay(10);
}
}

/**
 * @brief 
 * 设置pwm频率
 * 
 * @param timer 定时器对象
 * @param fre   频率
 */
void setpwmfre(hw_timer_t * timer,uint64_t fre){
  if(fre<1) fre=1;
  if(fre>100000) fre=100000;
  uint64_t times = 500000*40/fre;
  timerAlarmDisable(timer);
  timerAlarmWrite(timer, times, true);
  timerAlarmEnable(timer);
}

void setup() {
  Serial.begin(115200);
  // put your setup code here, to run once:
  pinMode(MOTO1pwm,OUTPUT);
  pinMode(MOTO2pwm,OUTPUT);
  pinMode(MOTO1dir,OUTPUT);
  pinMode(MOTO2dir,OUTPUT);
  pinMode(MOTO1en,OUTPUT);
  pinMode(MOTO2en,OUTPUT);
  pinMode(mmwavetrig_gpio,OUTPUT);//GPIO初始化
  digitalWrite(mmwavetrig_gpio,LOW);
  digitalWrite(MOTO2en,HIGH);                        //电机2取消使能，防止噪声导致抖动
  pref.begin("configuration");
  if(pref.isKey("xray_plusenum")){
    xray_plusenum=pref.getLong64("xray_plusenum");
  }
  if(pref.isKey("xray_fre")){
    xray_fre=pref.getLong64("xray_fre");
  }
  if(pref.isKey("yray_plusenum")){
    yray_plusenum=pref.getLong64("yray_plusenum");
  }
  if(pref.isKey("yray_fre")){
    yray_fre=pref.getLong64("yray_fre");
  }
  if(pref.isKey("yray_times")){
    yray_times=pref.getLong64("yray_times");
  }
  if(pref.isKey("sample_duty")){
    sample_duty=pref.getLong64("sample_duty");
  }                                             
  // Create semaphore to inform us when the timer has fired
  timer0Semaphore = xSemaphoreCreateBinary();
  timer1Semaphore = xSemaphoreCreateBinary();
  mmwavetriggeron = xSemaphoreCreateBinary();
  mmwavetriggeroff = xSemaphoreCreateBinary();
  // Use 1st timer of 4 (counted from zero).
  // Set 80 divider for prescaler (see ESP32 Technical Reference Manual for more
  // info).
  timer0 = timerBegin(0, 2, true);
  timer1 = timerBegin(1, 2, true);

  // Attach onTimer function to our timer.
  timerAttachInterrupt(timer0, &onTimer0, true);
  timerAttachInterrupt(timer1, &onTimer1, true);
  setpwmfre(timer0,xray_fre);
  setpwmfre(timer1,yray_fre);
  BLEDevice::init("bletest");
  // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pTxCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_TX,
                      BLECharacteristic::PROPERTY_WRITE|BLECharacteristic::PROPERTY_READ
                  );
  // pTxCharacteristic->addDescriptor(new BLE2902());
  pTxCharacteristic->setCallbacks(new CharacteristicTxCallback());
  std::string sentstate="0";
  pTxCharacteristic->setValue(sentstate);

  pRxCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_RX,
                      BLECharacteristic::PROPERTY_WRITE|BLECharacteristic::PROPERTY_READ
                    );
  pRxCharacteristic->setCallbacks(new CharacteristicRxCallback());
  bleRxjson.clear();
  bleRxjson["xray_plusenum"]=xray_plusenum;
  bleRxjson["xray_fre"]=xray_fre;
  bleRxjson["yray_plusenum"]=yray_plusenum;
  bleRxjson["yray_fre"]=yray_fre;
  bleRxjson["yray_times"]=yray_times;
  bleRxjson["sample_duty"]=sample_duty;
  char state_doc[measureJson(bleRxjson) + 1];
  serializeJson(bleRxjson, state_doc, measureJson(bleRxjson) + 1);
  pRxCharacteristic->setValue(state_doc);
  Serial.println(state_doc);
  // pRxCharacteristic->setValue(&state,1);
  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  xTaskCreatePinnedToCore(ble_handletask,    //具体实现的函数
                          "ble_handletask",  //任务名称
                          8192,       //堆栈大小
                          NULL,       //输入参数
                          1,          //任务优先级
                          NULL,       //
                          0           //核心  0\1
                          );
  xTaskCreatePinnedToCore(mmwavetrigtask,    //具体实现的函数
                          "mmwavetrigtask",  //任务名称
                          8192,       //堆栈大小
                          NULL,       //输入参数
                          1,          //任务优先级
                          NULL,       //
                          0           //核心  0\1
                          );
  // portENTER_CRITICAL_ISR(&timer0Mux);
  // xray_locationset=10000;
  // xray_workstate=true;
  // portEXIT_CRITICAL_ISR(&timer0Mux);
}

void loop() {
  if(now_worktimes<yray_times){
    if (xSemaphoreTake(timer1Semaphore, 0) == pdTRUE){
      Serial.printf("times:%lld   xlocation:%lld   ylocation:%lld",now_worktimes,xray_location,yray_location);
      if(now_worktimes%2==0){
        portENTER_CRITICAL_ISR(&timer0Mux);
        xray_locationset=xray_plusenum;
        xray_workstate=true;
        portEXIT_CRITICAL_ISR(&timer0Mux);
      }
      else{
        portENTER_CRITICAL_ISR(&timer0Mux);
        xray_locationset=-1*xray_plusenum;
        xray_workstate=true;
        portEXIT_CRITICAL_ISR(&timer0Mux);
      }
    }
    if (xSemaphoreTake(timer0Semaphore, 0) == pdTRUE){
      Serial.printf("times:%lld   xlocation:%lld   ylocation:%lld",now_worktimes,xray_location,yray_location);
      digitalWrite(MOTO2en,LOW);                        //电机2使能，防止噪声导致抖动
      portENTER_CRITICAL_ISR(&timer1Mux);
      yray_locationset=yray_plusenum;
      yray_workstate=true;
      portEXIT_CRITICAL_ISR(&timer1Mux);
    }
  }
  else{
    if(xray_location!=0){
      if (xSemaphoreTake(timer1Semaphore, 0) == pdTRUE){
        Serial.printf("times:%lld   xlocation:%lld   ylocation:%lld",now_worktimes,xray_location,yray_location);
        portENTER_CRITICAL_ISR(&timer0Mux);
        xray_locationset=-1*xray_location;
        xray_workstate=true;
        portEXIT_CRITICAL_ISR(&timer0Mux);
      }
    }
    if(yray_location!=0){
      if (xSemaphoreTake(timer0Semaphore, 0) == pdTRUE){
        Serial.printf("times:%lld   xlocation:%lld   ylocation:%lld",now_worktimes,xray_location,yray_location);
        digitalWrite(MOTO2en,LOW);                        //电机2使能，防止噪声导致抖动
        portENTER_CRITICAL_ISR(&timer1Mux);
        yray_locationset=-1*yray_location;
        yray_workstate=true;
        portEXIT_CRITICAL_ISR(&timer1Mux);
      }
    }
    if(xray_location==0&&yray_location==0){
      if (xSemaphoreTake(timer1Semaphore, 0) == pdTRUE){
        std::string sentstate="0";
        pTxCharacteristic->setValue(sentstate);
      }

    }
  }
  delay(100);
  // ets_delay_us(3);

}